/*
 * Double_Buffer.cpp
 *
 *  Created on: Mar 7, 2012
 *      Author: ajeffery
 */

#include "Double_Buffer.h"
#include "defs.h"
#include <native/mutex.h>
#include <native/cond.h>
#include <rtdk.h>
#include <stdint.h>



Double_Buffer::Double_Buffer() {
	rt_mutex_create(&mutex, NULL);
	rt_cond_create(&cond, NULL);

	for (int i = 0; i < 2; i++) {
		index[i] = 0;
		length[i] = 0;
		for (int j = 0; j < BUFFER_SIZE; j++) {
			status_t t;
			buffer[i][j] = t;
		}
	}



}

Double_Buffer::~Double_Buffer() {
	rt_cond_delete(&cond);
	rt_mutex_delete(&mutex);
}



int Double_Buffer::isFull(int buffId) {
	return length[buffId] == BUFFER_SIZE;
}

int Double_Buffer::isEmpty(int buffId) {

	return length[buffId] == 0;
}

void Double_Buffer::put(status_t * data){
	rt_printf("TEST put\n");
	rt_mutex_acquire(&mutex, TM_INFINITE);

	int i;
	for (i = 0; i < 2; i++) {
		while(isFull(i)) {
			rt_cond_wait(&cond, &mutex, TM_INFINITE);
		}

		int writeIndex = (index[i] + length[i]) % BUFFER_SIZE;

		buffer[i][writeIndex].position = data->position;
		buffer[i][writeIndex].dest = data->dest;
		for (int j=0; i < MAX_FLOORS; i++)
				buffer[i][writeIndex].current_calls[i] = data->current_calls[j];
		length[i]++;

		rt_cond_broadcast(&cond);

	}

	rt_mutex_release(&mutex);

}

status_t Double_Buffer::get(int buffId) {

	status_t data;
	rt_mutex_acquire(&mutex, TM_INFINITE);

	while(isEmpty(buffId)) {
		rt_cond_wait(&cond, &mutex, TM_INFINITE);
	}

	int readIndex = index[buffId];
	data.position = buffer[buffId][readIndex].position;
	data.dest = buffer[buffId][readIndex].dest;
	for (int j=0; j < MAX_FLOORS; j++)
		data.current_calls[j] = buffer[buffId][readIndex].current_calls[j];

	index[buffId] = (index[buffId] + 1) % BUFFER_SIZE;
	length[buffId]--;

	rt_cond_broadcast(&cond);
	rt_mutex_release(&mutex);

	return data;
}




